
/********************************************************************
* Description: motion.c
*   Main module initialisation and cleanup routines.
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************/
#include <stdio.h>
#include <stdarg.h>
#include "rtapi.h"		/* RTAPI realtime OS API */
#include "rtapi_app.h"		/* RTAPI realtime module decls */
#include "rtapi_string.h"       /* memset */
#include "hal.h"		/* decls for HAL implementation */
#include "emcmotglb.h"
#include "motion.h"
#include "motion_debug.h"
#include "motion_struct.h"
#include "mot_priv.h"
#include "math.h"
#include "cubicclass.h"
#include <float.h>
#include <stdlib.h>
#include <iostream>
// vtable signatures
#define VTKINS_VERSION VTKINEMATICS_VERSION1
#define VTP_VERSION    VTTP_VERSION1

// Mark strings for translation, but defer translation to userspace
#define _(s) (s)

#include "emcmotionclass.h"


//#define GET_MOTION_ERROR_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ERROR_BIT ? 1 : 0)
int GET_MOTION_ERROR_FLAG(unsigned short motionFlag)
{
    return motionFlag & EMCMOT_MOTION_ERROR_BIT ? 1 : 0;
}

//#define SET_MOTION_ERROR_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ERROR_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ERROR_BIT;
void SET_MOTION_ERROR_FLAG(int fl,unsigned short &motionFlag)
{
    if (fl) motionFlag |= EMCMOT_MOTION_ERROR_BIT; else motionFlag &= ~EMCMOT_MOTION_ERROR_BIT;
}

//#define GET_MOTION_COORD_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_COORD_BIT ? 1 : 0)
int GET_MOTION_COORD_FLAG(unsigned short motionFlag)
{
    return motionFlag & EMCMOT_MOTION_COORD_BIT ? 1 : 0;
}

//#define SET_MOTION_COORD_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_COORD_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_COORD_BIT;
void SET_MOTION_COORD_FLAG(int fl,unsigned short &motionFlag)
{
    if (fl) motionFlag |= EMCMOT_MOTION_COORD_BIT; else motionFlag &= ~EMCMOT_MOTION_COORD_BIT;
}

//#define GET_MOTION_TELEOP_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_TELEOP_BIT ? 1 : 0)
int GET_MOTION_TELEOP_FLAG(unsigned short motionFlag)
{
    return motionFlag & EMCMOT_MOTION_TELEOP_BIT ? 1 : 0;
}

//#define SET_MOTION_TELEOP_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_TELEOP_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_TELEOP_BIT;
void SET_MOTION_TELEOP_FLAG(int fl,unsigned short &motionFlag)
{
    if (fl) motionFlag |= EMCMOT_MOTION_TELEOP_BIT; else motionFlag &= ~EMCMOT_MOTION_TELEOP_BIT;
}
//#define GET_MOTION_INPOS_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0)
int GET_MOTION_INPOS_FLAG(unsigned short motionFlag)
{
    return motionFlag & EMCMOT_MOTION_INPOS_BIT ? 1 : 0;
}
//#define SET_MOTION_INPOS_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_INPOS_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_INPOS_BIT;
void SET_MOTION_INPOS_FLAG(int fl,unsigned short &motionFlag)
{
    if (fl) motionFlag |= EMCMOT_MOTION_INPOS_BIT; else motionFlag &= ~EMCMOT_MOTION_INPOS_BIT;
}
//#define GET_MOTION_ENABLE_FLAG() (emcmotStatus->motionFlag & EMCMOT_MOTION_ENABLE_BIT ? 1 : 0)
int GET_MOTION_ENABLE_FLAG(unsigned short motionFlag)
{
    return motionFlag & EMCMOT_MOTION_ENABLE_BIT ? 1 : 0;
}
//#define SET_MOTION_ENABLE_FLAG(fl) if (fl) emcmotStatus->motionFlag |= EMCMOT_MOTION_ENABLE_BIT; else emcmotStatus->motionFlag &= ~EMCMOT_MOTION_ENABLE_BIT;
void SET_MOTION_ENABLE_FLAG(int fl,unsigned short &motionFlag)
{
    if (fl) motionFlag |= EMCMOT_MOTION_ENABLE_BIT; else motionFlag &= ~EMCMOT_MOTION_ENABLE_BIT;
}

EmcMotionClass::EmcMotionClass()
{

}


/***********************************************************************
*                    KERNEL MODULE PARAMETERS                          *
************************************************************************/

static int key = DEFAULT_MOTION_SHMEM_KEY;		/* the shared memory key, default value */

/* module information */
/* register symbols to be modified by insmod
   see "Linux Device Drivers", Alessandro Rubini, p. 385
   (p.42-44 in 2nd edition) */
//MODULE_AUTHOR("Matt Shaver/John Kasunich");
//MODULE_DESCRIPTION("Motion Controller for EMC");
//MODULE_LICENSE("GPL");

/*! \todo FIXME - find a better way to do this */
//int DEBUG_MOTION = 0;
//RTAPI_MP_INT(DEBUG_MOTION, "debug motion");

/* RTAPI shmem key - for comms with higher level user space stuff */
//RTAPI_MP_INT(key, "shared memory key");
static long base_period_nsec = 0;	/* fastest thread period */
//RTAPI_MP_LONG(base_period_nsec, "fastest thread period (nsecs)");
static int base_cpu = -1;		/* explicitly bind to CPU */
//RTAPI_MP_INT(base_cpu, "CPU of base thread");
int base_thread_fp = 0;	/* default is no floating point in base thread */
//RTAPI_MP_INT(base_thread_fp, "floating point in base thread?");
static long servo_period_nsec = 1000000;	/* servo thread period */
//RTAPI_MP_LONG(servo_period_nsec, "servo thread period (nsecs)");
static int servo_cpu = -1;		/* explicitly bind to CPU */
//RTAPI_MP_INT(servo_cpu, "CPU of servo thread");
long traj_period_nsec = 0;	/* trajectory planner period */
//RTAPI_MP_LONG(traj_period_nsec, "trajectory planner period (nsecs)");
int num_joints = EMCMOT_MAX_JOINTS;	/* default number of joints present */
//RTAPI_MP_INT(num_joints, "number of joints");
int num_dio = 4;			/* default number of motion synched DIO */
//RTAPI_MP_INT(num_dio, "number of digital inputs/outputs");
int num_aio = 4;			/* default number of motion synched AIO */
//RTAPI_MP_INT(num_aio, "number of analog inputs/outputs");
static char *kins = "trivkins";
//RTAPI_MP_STRING(kins, "kinematics vtable name");
static char *tp = "tp";
//RTAPI_MP_STRING(tp, "tp vtable name");

/***********************************************************************
*                  GLOBAL VARIABLE DEFINITIONS                         *
************************************************************************/

/* pointer to emcmot_hal_data_t struct in HAL shmem, with all HAL data */
emcmot_hal_data_t *emcmot_hal_data = 0;

/* pointer to joint data */
emcmot_joint_t *joints= 0;

/* Joints moved to HAL shared memory */
#if 0 // #ifndef STRUCTS_IN_SHMEM
/* allocate array for joint data */
emcmot_joint_t joint_array[EMCMOT_MAX_JOINTS];
#endif

int mot_comp_id;	/* component ID for motion module */
//int first_pass = 1;	/* used to set initial conditions */
//int kinType = 0;

/*
  Principles of communication:

  Data is copied in or out from the various types of comm mechanisms:
  mbuff mapped memory for Linux/RT-Linux, or OS shared memory for Unixes.

  emcmotStruct is ptr to this memory.

  emcmotCommand points to emcmotStruct->command,
  emcmotStatus points to emcmotStruct->status,
  emcmotError points to emcmotStruct->error, and
 */
//emcmot_struct_t *emcmotStruct = 0;
/* ptrs to either buffered copies or direct memory for
   command and status */
struct emcmot_command_t *emcmotCommand[D_MAX_CARVE_NUM];
struct emcmot_status_t *emcmotStatus[D_MAX_CARVE_NUM];
struct emcmot_config_t *emcmotConfig[D_MAX_CARVE_NUM];
struct emcmot_debug_t *emcmotDebug[D_MAX_CARVE_NUM];
TP_STRUCT *emcmotPrimQueue[D_MAX_CARVE_NUM]; // primary planner + queues
TP_STRUCT *emcmotAltQueue[D_MAX_CARVE_NUM]; // alternate planner + queues

// emcmotQueue: this was formerly &emcmotDebug->queue
TP_STRUCT *emcmotQueue[D_MAX_CARVE_NUM];     // current planner queue

struct emcmot_internal_t *emcmotInternal[D_MAX_CARVE_NUM];
struct emcmot_error_t *emcmotError[D_MAX_CARVE_NUM];	/* unused for RT_FIFO */

void rtapi_print_msg(msg_level_t level, const char *fmt, ...)
{
    printf(fmt);
}

/***********************************************************************
*                  LOCAL VARIABLE DECLARATIONS                         *
************************************************************************/

/* RTAPI shmem ID - for comms with higher level user space stuff */
static int emc_shmem_id;	/* the shared memory ID */

/***********************************************************************
*                   LOCAL FUNCTION PROTOTYPES                          *
************************************************************************/

/* init_hal_io() exports HAL pins and parameters making data from
   the realtime control module visible and usable by the world
*/
static int init_hal_io(void);

/* functions called by init_hal_io() */
static int export_jointNew(int num, joint_hal_t * addr);

/* init_comm_buffers() allocates and initializes the command,
   status, and error buffers used to communicate witht the user
   space parts of emc.
*/
static int init_comm_buffers(emcmot_config_t configIn, int robotIdIn);

/* functions called by init_comm_buffers() */

/* init_threads() creates realtime threads, exports functions to
   do the realtime control, and adds the functions to the threads.
*/
static int init_threads(int robotIdIn);

/* functions called by init_threads() */
//int setTrajCycleTime(double secs);
//int setServoCycleTime(double secs);

// init the shared state handling between tp and motion
static int init_shared(tp_shared_t *tps,
               struct emcmot_config_t *cfg,
               struct emcmot_status_t *status,
               emcmot_debug_t *dbg,
               emcmot_joint_t *joint,
               emcmot_hal_data_t *hal);
/***********************************************************************
*                     PUBLIC FUNCTION CODE                             *
************************************************************************/

void emcmot_config_change(int robotIdIn)
{
    if (emcmotConfig[robotIdIn]->head == emcmotConfig[robotIdIn]->tail) {
    emcmotConfig[robotIdIn]->config_num++;
    emcmotStatus[robotIdIn]->config_num = emcmotConfig[robotIdIn]->config_num;
    emcmotConfig[robotIdIn]->head++;
    }
}

void reportError(const char *fmt, ...)
{
//    va_list args;

//    va_start(args, fmt);
//    emcmotErrorPutfv(emcmotError[robotIdIn], fmt, args);
//    va_end(args);
}

#ifndef va_copy
#define va_copy(dest, src) ((dest)=(src))
#endif

static rtapi_msg_handler_t old_handler = NULL;
static void emc_message_handler(msg_level_t level, const char *fmt, va_list ap)
{
//    va_list apc;
//    va_copy(apc, ap);
//    if(level == RTAPI_MSG_ERR) emcmotErrorPutfv(emcmotError, fmt, apc);
//    if(old_handler) old_handler(level, fmt, ap);
//    va_end(apc);
}

int EmcMotionClass::rtapi_app_main_intial(struct emcmot_config_t configIn, int robotIdIn)
{
    int retval;
    // rtapi_set_msg_level(RTAPI_MSG_DBG);
//    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() starting...\n");

    /* set flag */
//    first_pass = 1;
    /* connect to the HAL and RTAPI */
    mot_comp_id = hal_init("motmod");
//    if (mot_comp_id < 0) {
////    rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: hal_init() failed\n"));
//    return -1;
//    }
    if (( num_joints < 1 ) || ( num_joints > EMCMOT_MAX_JOINTS )) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//        _("MOTION: num_joints is %d, must be between 1 and %d\n"),
//        num_joints, EMCMOT_MAX_JOINTS);
    return -1;
    }

    if (( num_dio < 1 ) || ( num_dio > EMCMOT_MAX_DIO )) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//        _("MOTION: num_dio is %d, must be between 1 and %d\n"),
//        num_dio, EMCMOT_MAX_DIO);
    return -1;
    }

    if (( num_aio < 1 ) || ( num_aio > EMCMOT_MAX_AIO )) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//        _("MOTION: num_aio is %d, must be between 1 and %d\n"),
//        num_aio, EMCMOT_MAX_AIO);
    return -1;
    }

    /* initialize/export HAL pins and parameters */
    retval = init_hal_io();
//    if (retval != 0) {
////    rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_hal_io() failed\n"));
////    hal_exit(mot_comp_id);
//    return -1;
//    }

    /* allocate/initialize user space comm buffers (cmd/status/err) */
    retval = init_comm_buffers(configIn,robotIdIn);
//    if (retval != 0) {
////    rtapi_print_msg(RTAPI_MSG_ERR,
////        _("MOTION: init_comm_buffers() failed\n"));
//    hal_exit(mot_comp_id);
//    return -1;
//    }

    /* set up for realtime execution of code */
//    retval = init_threads();
//    if (retval != 0) {
////    rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_threads() failed\n"));
//    hal_exit(mot_comp_id);
//    return -1;
//    }

//    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() complete\n");

//    hal_ready(mot_comp_id);

//    old_handler = rtapi_get_msg_handler();
//    rtapi_set_msg_handler(emc_message_handler);
    return 0;
}
//int EmcMotionClass::rtapi_app_main_intial(void)
//{
////    int retval;
////    // rtapi_set_msg_level(RTAPI_MSG_DBG);
////    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() starting...\n");

////    /* set flag */
////    first_pass = 1;
////    /* connect to the HAL and RTAPI */
////    mot_comp_id = hal_init("motmod");
////    if (mot_comp_id < 0) {
//////    rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: hal_init() failed\n"));
////    return -1;
////    }
////    if (( num_joints < 1 ) || ( num_joints > EMCMOT_MAX_JOINTS )) {
////    rtapi_print_msg(RTAPI_MSG_ERR,
////        _("MOTION: num_joints is %d, must be between 1 and %d\n"),
////        num_joints, EMCMOT_MAX_JOINTS);
////    return -1;
////    }

////    if (( num_dio < 1 ) || ( num_dio > EMCMOT_MAX_DIO )) {
////    rtapi_print_msg(RTAPI_MSG_ERR,
////        _("MOTION: num_dio is %d, must be between 1 and %d\n"),
////        num_dio, EMCMOT_MAX_DIO);
////    return -1;
////    }

////    if (( num_aio < 1 ) || ( num_aio > EMCMOT_MAX_AIO )) {
////    rtapi_print_msg(RTAPI_MSG_ERR,
////        _("MOTION: num_aio is %d, must be between 1 and %d\n"),
////        num_aio, EMCMOT_MAX_AIO);
////    return -1;
////    }

////    /* initialize/export HAL pins and parameters */
////    retval = init_hal_io();
////    if (retval != 0) {
////    rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_hal_io() failed\n"));
////    hal_exit(mot_comp_id);
////    return -1;
////    }

////    /* allocate/initialize user space comm buffers (cmd/status/err) */
////    retval = init_comm_buffers();
////    if (retval != 0) {
////    rtapi_print_msg(RTAPI_MSG_ERR,
////        _("MOTION: init_comm_buffers() failed\n"));
////    hal_exit(mot_comp_id);
////    return -1;
////    }

////    /* set up for realtime execution of code */
////    retval = init_threads();
////    if (retval != 0) {
////    rtapi_print_msg(RTAPI_MSG_ERR, _("MOTION: init_threads() failed\n"));
////    hal_exit(mot_comp_id);
////    return -1;
////    }

////    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_module() complete\n");

////    hal_ready(mot_comp_id);

////    old_handler = rtapi_get_msg_handler();
////    rtapi_set_msg_handler(emc_message_handler);
//    return 0;
//}

void EmcMotionClass::rtapi_app_exit_end(void)
{
    int retval;

//    rtapi_set_msg_handler(old_handler);

//    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: cleanup_module() started.\n");

//    retval = hal_stop_threads();
//    if (retval < 0) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//        _("MOTION: hal_stop_threads() failed, returned %d\n"), retval);
//    }

    // release the kinematics vtable
//    hal_unreference_vtable(emcmotConfig[robotIdIn]->kins_vid);

    // release the tp vtable
//    hal_unreference_vtable(emcmotConfig[robotIdIn]->tp_vid);

    /* free shared memory */
//    retval = rtapi_shmem_delete(emc_shmem_id, mot_comp_id);
//    if (retval < 0) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//        _("MOTION: rtapi_shmem_delete() failed, returned %d\n"), retval);
//    }
//    /* disconnect from HAL and RTAPI */
//    retval = hal_exit(mot_comp_id);
//    if (retval < 0) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//        _("MOTION: hal_exit() failed, returned %d\n"), retval);
//    }
//    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: cleanup_module() finished.\n");
}

/***********************************************************************
*                         LOCAL FUNCTION CODE                          *
************************************************************************/

/* init_hal_io() exports HAL pins and parameters making data from
   the realtime control module visible and usable by the world
*/
static int init_hal_io(void)
{
    int n, retval;
    joint_hal_t *joint_data;

//    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_hal_io() starting...\n");

    /* allocate shared memory for machine data */
    emcmot_hal_data = (emcmot_hal_data_t *)malloc(sizeof(emcmot_hal_data_t));


    if (joints == 0)
    {
        /* allocate shared memory for joint data */
        joints = (emcmot_joint_t *)malloc(sizeof(emcmot_joint_t) * EMCMOT_MAX_JOINTS*D_MAX_CARVE_NUM);
        /* Clear joints memory */
        memset(joints, 0, sizeof(emcmot_joint_t) * EMCMOT_MAX_JOINTS*D_MAX_CARVE_NUM);
    }



//    /* export machine wide hal pins */
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->probe_input), mot_comp_id, "motion.probe-input")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->spindle_index_enable), mot_comp_id, "motion.spindle-index-enable")) < 0) goto error;

    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_on), mot_comp_id, "motion.spindle-on")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_forward), mot_comp_id, "motion.spindle-forward")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_reverse), mot_comp_id, "motion.spindle-reverse")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_brake), mot_comp_id, "motion.spindle-brake")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out), mot_comp_id, "motion.spindle-speed-out")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_abs), mot_comp_id, "motion.spindle-speed-out-abs")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps), mot_comp_id, "motion.spindle-speed-out-rps")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_out_rps_abs), mot_comp_id, "motion.spindle-speed-out-rps-abs")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_speed_cmd_rps), mot_comp_id, "motion.spindle-speed-cmd-rps")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_inhibit), mot_comp_id, "motion.spindle-inhibit")) < 0) goto error;
    *(emcmot_hal_data->spindle_inhibit) = 0;

    // spindle orient pins
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_angle), mot_comp_id, "motion.spindle-orient-angle")) < 0) goto error;
    if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient_mode), mot_comp_id, "motion.spindle-orient-mode")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_orient), mot_comp_id, "motion.spindle-orient")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->spindle_locked), mot_comp_id, "motion.spindle-locked")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_oriented), mot_comp_id, "motion.spindle-is-oriented")) < 0) goto error;
    if ((retval = hal_pin_s32_newf(HAL_IN, &(emcmot_hal_data->spindle_orient_fault), mot_comp_id, "motion.spindle-orient-fault")) < 0) goto error;
    *(emcmot_hal_data->spindle_orient_angle) = 0.0;
    *(emcmot_hal_data->spindle_orient_mode) = 0;
    *(emcmot_hal_data->spindle_orient) = 0;


//    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->inpos_output), mot_comp_id, "motion.motion-inpos")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_revs), mot_comp_id, "motion.spindle-revs")) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->spindle_speed_in), mot_comp_id, "motion.spindle-speed-in")) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->spindle_is_atspeed), mot_comp_id, "motion.spindle-at-speed")) < 0) goto error;
    *emcmot_hal_data->spindle_is_atspeed = 1;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->adaptive_feed), mot_comp_id, "motion.adaptive-feed")) < 0) goto error;
    *(emcmot_hal_data->adaptive_feed) = 1.0;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_hold), mot_comp_id, "motion.feed-hold")) < 0) goto error;
    *(emcmot_hal_data->feed_hold) = 0;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->feed_inhibit), mot_comp_id, "motion.feed-inhibit")) < 0) goto error;
    *(emcmot_hal_data->feed_inhibit) = 0;

    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->enable), mot_comp_id, "motion.enable")) < 0) goto error;

    /* export motion-synched digital output pins */
    /* export motion-synched digital output io pins for compatibility with io signals */
    /* export motion digital input pins */
    for (n = 0; n < num_dio; n++) {
    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->synch_do[n]), mot_comp_id, "motion.digital-out-%02d", n)) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->synch_di[n]), mot_comp_id, "motion.digital-in-%02d", n)) < 0) goto error;
    if ((retval = hal_pin_bit_newf(HAL_IO, &(emcmot_hal_data->synch_do_io[n]), mot_comp_id, "motion.digital-out-io-%02d", n)) < 0) goto error;
    }

    /* export motion-synched analog output pins */
    /* export motion-synched analog output io pins for compatibility with io signals */
    /* export motion analog input pins */
    for (n = 0; n < num_aio; n++) {
    if ((retval = hal_pin_float_newf(HAL_OUT, &(emcmot_hal_data->analog_output[n]), mot_comp_id, "motion.analog-out-%02d", n)) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->analog_input[n]), mot_comp_id, "motion.analog-in-%02d", n)) < 0) goto error;
    if ((retval = hal_pin_float_newf(HAL_IO, &(emcmot_hal_data->analog_output_io[n]), mot_comp_id, "motion.analog-out-io-%02d", n)) < 0) goto error;
    }

    /* export machine wide hal parameters */
    retval =
    hal_pin_bit_new("motion.motion-enabled", HAL_OUT, &(emcmot_hal_data->motion_enabled),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_new("motion.in-position", HAL_OUT, &(emcmot_hal_data->in_position),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_new("motion.coord-mode", HAL_OUT, &(emcmot_hal_data->coord_mode),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_new("motion.teleop-mode", HAL_OUT, &(emcmot_hal_data->teleop_mode),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_new("motion.coord-error", HAL_OUT, &(emcmot_hal_data->coord_error),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_new("motion.on-soft-limit", HAL_OUT, &(emcmot_hal_data->on_soft_limit),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_new("motion.current-vel", HAL_OUT, &(emcmot_hal_data->current_vel),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_new("motion.requested-vel", HAL_OUT, &(emcmot_hal_data->requested_vel),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_new("motion.distance-to-go", HAL_OUT, &(emcmot_hal_data->distance_to_go),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_s32_new("motion.program-line", HAL_OUT, &(emcmot_hal_data->program_line),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    /* export debug parameters */
    /* these can be used to view any internal variable, simply change a line
       in control.c:output_to_hal() and recompile */
    retval =
    hal_param_bit_new("motion.debug-bit-0", HAL_RO, &(emcmot_hal_data->debug_bit_0),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_param_bit_new("motion.debug-bit-1", HAL_RO, &(emcmot_hal_data->debug_bit_1),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }

    retval =
    hal_param_float_new("motion.debug-float-0", HAL_RO, &(emcmot_hal_data->debug_float_0),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_param_float_new("motion.debug-float-1", HAL_RO, &(emcmot_hal_data->debug_float_1),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }

    retval =
    hal_param_float_new("motion.debug-float-2", HAL_RO, &(emcmot_hal_data->debug_float_2),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }

    retval =
    hal_param_float_new("motion.debug-float-3", HAL_RO, &(emcmot_hal_data->debug_float_3),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }

    retval =
    hal_param_s32_new("motion.debug-s32-0", HAL_RO, &(emcmot_hal_data->debug_s32_0),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_param_s32_new("motion.debug-s32-1", HAL_RO, &(emcmot_hal_data->debug_s32_1),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }

    // FIXME - debug only, remove later
    // export HAL parameters for some trajectory planner internal variables
    // so they can be scoped
    retval =
    hal_param_float_new("traj.pos_out", HAL_RO, &(emcmot_hal_data->traj_pos_out),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_param_float_new("traj.vel_out", HAL_RO, &(emcmot_hal_data->traj_vel_out),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_param_u32_new("traj.active_tc", HAL_RO, &(emcmot_hal_data->traj_active_tc),
    mot_comp_id);
    if (retval != 0) {
    return retval;
    }
    for ( n = 0 ; n < 4 ; n++ ) {
    retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_pos[n]), mot_comp_id, "tc.%d.pos", n);
    if (retval != 0) {
        return retval;
    }
    retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_vel[n]), mot_comp_id, "tc.%d.vel", n);
    if (retval != 0) {
        return retval;
    }
    retval = hal_param_float_newf(HAL_RO, &(emcmot_hal_data->tc_acc[n]), mot_comp_id, "tc.%d.acc", n);
    if (retval != 0) {
        return retval;
    }
    }
    // end of exporting trajectory planner internals

    // export timing related HAL parameters so they can be scoped
    retval =
    hal_param_u32_new("motion.servo.last-period", HAL_RO, &(emcmot_hal_data->last_period), mot_comp_id);
    if (retval != 0) {
    return retval;
    }
#ifdef HAVE_CPU_KHZ
    retval =
    hal_param_float_new("motion.servo.last-period-ns", HAL_RO, &(emcmot_hal_data->last_period_ns), mot_comp_id);
    if (retval != 0) {
    return retval;
    }
#endif
    retval =
    hal_param_u32_new("motion.servo.overruns", HAL_RW, &(emcmot_hal_data->overruns), mot_comp_id);
    if (retval != 0) {
    return retval;
    }

    retval = hal_pin_float_new("motion.tooloffset.x", HAL_OUT, &(emcmot_hal_data->tooloffset_x), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.y", HAL_OUT, &(emcmot_hal_data->tooloffset_y), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.z", HAL_OUT, &(emcmot_hal_data->tooloffset_z), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.a", HAL_OUT, &(emcmot_hal_data->tooloffset_a), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.b", HAL_OUT, &(emcmot_hal_data->tooloffset_b), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.c", HAL_OUT, &(emcmot_hal_data->tooloffset_c), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.u", HAL_OUT, &(emcmot_hal_data->tooloffset_u), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.v", HAL_OUT, &(emcmot_hal_data->tooloffset_v), mot_comp_id);
    if (retval != 0) {
        return retval;
    }
    retval = hal_pin_float_new("motion.tooloffset.w", HAL_OUT, &(emcmot_hal_data->tooloffset_w), mot_comp_id);
    if (retval != 0) {
        return retval;
    }

    if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->pause_state),
                   mot_comp_id, "motion.pause-state")) < 0) return retval;

    // feedhold-offset related pins
    if ((retval = hal_pin_bit_newf(HAL_IN, &(emcmot_hal_data->pause_offset_enable),
                   mot_comp_id, "motion.pause-offset-enable")) < 0) return retval;

    if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->paused_at_motion_type),
                   mot_comp_id, "motion.paused-at-motion")) < 0) return retval;

    if ((retval = hal_pin_s32_newf(HAL_OUT, &(emcmot_hal_data->current_motion_type),
                   mot_comp_id, "motion.current-motion")) < 0) return retval;

    if ((retval = hal_pin_bit_newf(HAL_OUT, &(emcmot_hal_data->pause_offset_in_range),
                   mot_comp_id, "motion.pause-offset-in-range")) < 0) return retval;


    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_jog_vel),
                     mot_comp_id, "motion.pause-jog-feed")) < 0) return retval;

    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_x),
                     mot_comp_id, "motion.pause-offset-x")) < 0) return retval;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_y),
                     mot_comp_id, "motion.pause-offset-y")) < 0) return retval;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_z),
                     mot_comp_id, "motion.pause-offset-z")) < 0) return retval;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_a),
                     mot_comp_id, "motion.pause-offset-a")) < 0) return retval;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_b),
                     mot_comp_id, "motion.pause-offset-b")) < 0) return retval;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_c),
                     mot_comp_id, "motion.pause-offset-c")) < 0) return retval;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_u),
                     mot_comp_id, "motion.pause-offset-u")) < 0) return retval;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_v),
                     mot_comp_id, "motion.pause-offset-v")) < 0) return retval;
    if ((retval = hal_pin_float_newf(HAL_IN, &(emcmot_hal_data->pause_offset_w),
                     mot_comp_id, "motion.pause-offset-w")) < 0) return retval;


    /* initialize machine wide pins and parameters */
    *(emcmot_hal_data->probe_input) = 0;
    /* default value of enable is TRUE, so simple machines
       can leave it disconnected */
    *(emcmot_hal_data->enable) = 1;

    /* motion synched dio, init to not enabled */
    for (n = 0; n < num_dio; n++) {
     *(emcmot_hal_data->synch_do[n]) = 0;
     *(emcmot_hal_data->synch_di[n]) = 0;
     *(emcmot_hal_data->synch_do_io[n]) = 0;
    }

    for (n = 0; n < num_aio; n++) {
     *(emcmot_hal_data->analog_output[n]) = 0.0;
     *(emcmot_hal_data->analog_input[n]) = 0.0;
     *(emcmot_hal_data->analog_output_io[n]) = 0.0;
    }

    /*! \todo FIXME - these don't really need initialized, since they are written
       with data from the emcmotStatus struct */
    *(emcmot_hal_data->motion_enabled) = 0;
    *(emcmot_hal_data->in_position) = 0;
    *(emcmot_hal_data->coord_mode) = 0;
    *(emcmot_hal_data->teleop_mode) = 0;
    *(emcmot_hal_data->coord_error) = 0;
    *(emcmot_hal_data->on_soft_limit) = 0;

    /* init debug parameters */
    emcmot_hal_data->debug_bit_0 = 0;
    emcmot_hal_data->debug_bit_1 = 0;
    emcmot_hal_data->debug_float_0 = 0.0;
    emcmot_hal_data->debug_float_1 = 0.0;
    emcmot_hal_data->debug_float_2 = 0.0;
    emcmot_hal_data->debug_float_3 = 0.0;

    emcmot_hal_data->overruns = 0;
    emcmot_hal_data->last_period = 0;

    /* export joint pins and parameters */
    for (n = 0; n < num_joints; n++)
    {
    /* point to axis data */
    joint_data = &(emcmot_hal_data->joint[n]);
    /* export all vars */
        retval = export_jointNew(n, joint_data);
    }
//    if (retval != 0) {
////        rtapi_print_msg(RTAPI_MSG_ERR,
////        _("MOTION: joint %d pin/param export failed\n"), n);
//        return -1;
//    }
//    /* init axis pins and parameters */
//    /* FIXME - struct members are in a state of flux - make sure to
//       update this - most won't need initing anyway */
//    *(joint_data->amp_enable) = 0;
//    *(joint_data->home_state) = 0;
//    /* We'll init the index model to EXT_ENCODER_INDEX_MODEL_RAW for now,
//       because it is always supported. */
//    }

    /* Done! */
//    rtapi_print_msg(RTAPI_MSG_INFO,
//    "MOTION: init_hal_io() complete, %d axes.\n", n);
    return 0;

    error:
    return retval;

}


static int export_jointNew(int num, joint_hal_t * addr)
{
    int retval, msg;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
//    msg = rtapi_get_msg_level();
//    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* export joint pins */ //FIXME-AJ: changing these will bork configs, still we should do it
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_cmd), mot_comp_id, "axis.%d.joint-pos-cmd", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->joint_pos_fb), mot_comp_id, "axis.%d.joint-pos-fb", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->motor_pos_cmd), mot_comp_id, "axis.%d.motor-pos-cmd", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->motor_offset), mot_comp_id, "axis.%d.motor-offset", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_IN, &(addr->motor_pos_fb), mot_comp_id, "axis.%d.motor-pos-fb", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_IN, &(addr->pos_lim_sw), mot_comp_id, "axis.%d.pos-lim-sw-in", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_IN, &(addr->neg_lim_sw), mot_comp_id, "axis.%d.neg-lim-sw-in", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_IN, &(addr->home_sw), mot_comp_id, "axis.%d.home-sw-in", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_IO, &(addr->index_enable), mot_comp_id, "axis.%d.index-enable", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->amp_enable), mot_comp_id, "axis.%d.amp-enable-out", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_IN, &(addr->amp_fault), mot_comp_id, "axis.%d.amp-fault-in", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_s32_newf(HAL_IN, &(addr->jog_counts), mot_comp_id, "axis.%d.jog-counts", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_enable), mot_comp_id, "axis.%d.jog-enable", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_float_newf(HAL_IN, &(addr->jog_scale), mot_comp_id, "axis.%d.jog-scale", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_IN, &(addr->jog_vel_mode), mot_comp_id, "axis.%d.jog-vel-mode", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->homing), mot_comp_id, "axis.%d.homing", num);
    if (retval != 0) {
    return retval;
    }
    /* export joint parameters */ //FIXME-AJ: changing these to joints will break configs.
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->coarse_pos_cmd),
    mot_comp_id, "axis.%d.coarse-pos-cmd", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->joint_vel_cmd), mot_comp_id, "axis.%d.joint-vel-cmd", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->backlash_corr), mot_comp_id, "axis.%d.backlash-corr", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->backlash_filt), mot_comp_id, "axis.%d.backlash-filt", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->backlash_vel), mot_comp_id, "axis.%d.backlash-vel", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_float_newf(HAL_OUT, &(addr->f_error), mot_comp_id, "axis.%d.f-error", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->f_error_lim), mot_comp_id, "axis.%d.f-error-lim", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->free_pos_cmd), mot_comp_id, "axis.%d.free-pos-cmd", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_float_newf(HAL_OUT, &(addr->free_vel_lim), mot_comp_id, "axis.%d.free-vel-lim", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_newf(HAL_OUT, &(addr->free_tp_enable), mot_comp_id, "axis.%d.free-tp-enable", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_newf(HAL_OUT, &(addr->kb_jog_active), mot_comp_id, "axis.%d.kb-jog-active", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_newf(HAL_OUT, &(addr->wheel_jog_active), mot_comp_id, "axis.%d.wheel-jog-active", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->active), mot_comp_id, "axis.%d.active", num);
    if (retval != 0) {
    return retval;
    }
    retval =
    hal_pin_bit_newf(HAL_OUT, &(addr->in_position), mot_comp_id, "axis.%d.in-position", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->error), mot_comp_id, "axis.%d.error", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->phl), mot_comp_id, "axis.%d.pos-hard-limit", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->nhl), mot_comp_id, "axis.%d.neg-hard-limit", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->homed), mot_comp_id, "axis.%d.homed", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->f_errored), mot_comp_id, "axis.%d.f-errored", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_bit_newf(HAL_OUT, &(addr->faulted), mot_comp_id, "axis.%d.faulted", num);
    if (retval != 0) {
    return retval;
    }
    retval = hal_pin_s32_newf(HAL_OUT, &(addr->home_state), mot_comp_id, "axis.%d.home-state", num);
    if (retval != 0) {
    return retval;
    }
    if(num >=3 && num <= 5) {
        // for rotaries only...
        retval = hal_pin_bit_newf(HAL_OUT, &(addr->unlock), mot_comp_id, "axis.%d.unlock", num);
        if (retval != 0) return retval;
        retval = hal_pin_bit_newf(HAL_IN, &(addr->is_unlocked), mot_comp_id, "axis.%d.is-unlocked", num);
//        *addr->is_unlocked=0;//hualei
        if (retval != 0) return retval;
    }
    return 0;
}


/* init_comm_buffers() allocates and initializes the command,
   status, and error buffers used to communicate with the user
   space parts of emc.
*/
static int init_comm_buffers(struct emcmot_config_t configIn,int robotIdIn)
{
    int joint_num, n;
    emcmot_joint_t *joint;
    int retval;

//    rtapi_print_msg(RTAPI_MSG_INFO,
//    "MOTION: init_comm_buffers() starting...\n");

    struct emcmot_struct_t *emcmotStruct;
    emcmotStruct = 0;
    emcmotDebug[robotIdIn] = 0;
    emcmotStatus[robotIdIn] = 0;
    emcmotCommand[robotIdIn] = 0;
    emcmotConfig[robotIdIn] = 0;


//    /* allocate and initialize the shared memory structure */
//    emc_shmem_id = rtapi_shmem_new(key, mot_comp_id, sizeof(emcmot_struct_t));
//    if (emc_shmem_id < 0) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//        "MOTION: rtapi_shmem_new failed, returned %d\n", emc_shmem_id);
//    return -1;
//    }
//    retval = rtapi_shmem_getptr(emc_shmem_id, (void **) &emcmotStruct);
//    if (retval < 0) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//        "MOTION: rtapi_shmem_getptr failed, returned %d\n", retval);
//    return -1;
//    }

    emcmotStruct = (emcmot_struct_t *)malloc(sizeof(emcmot_struct_t));
//    emcmotStruct->command = malloc(sizeof(emcmot_command_t));
//    emcmotStruct->status = (emcmot_status_t )malloc(sizeof(emcmot_status_t));
//    emcmotStruct->config = (emcmot_config_t )malloc(sizeof(emcmot_config_t));
//    emcmotStruct->internal = (emcmot_internal_t )malloc(sizeof(emcmot_internal_t));
//    emcmotStruct->error = (emcmot_error_t )malloc(sizeof(emcmot_error_t));
//    emcmotStruct->debug = (emcmot_debug_t )malloc(sizeof(emcmot_debug_t));


    /* zero shared memory before doing anything else. */
    memset(emcmotStruct, 0, sizeof(emcmot_struct_t));

    /* we'll reference emcmotStruct directly */
    emcmotCommand[robotIdIn] = &emcmotStruct->command;
    emcmotStatus[robotIdIn] = &emcmotStruct->status;
    emcmotConfig[robotIdIn] = &emcmotStruct->config;



    // bind kinematics vtable
//    emcmotConfig[robotIdIn]->kins_vid = hal_reference_vtable(kins, VTKINS_VERSION,
//                          (void **)&emcmotConfig[robotIdIn]->vtk);
//    if (emcmotConfig[robotIdIn]->kins_vid < 0) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//            "MOTION: hal_reference_vtable(%s,%d) failed: %d\n",
//            kins, VTKINS_VERSION, emcmotConfig[robotIdIn]->kins_vid);
//    return -1;
//    }

    /* record the kinematics type of the machine */
//    kinType = emcmotConfig[robotIdIn]->vtk->kinematicsType();

    // bind the tp vtable
//    emcmotConfig[robotIdIn]->tp_vid = hal_reference_vtable(tp, VTP_VERSION,
//                        (void **)&emcmotConfig[robotIdIn]->vtp);
//    if (emcmotConfig[robotIdIn]->tp_vid < 0) {
//    rtapi_print_msg(RTAPI_MSG_ERR,
//            "MOTION: hal_reference_vtable(%s,%d) failed: %d\n",
//            tp, VTP_VERSION, emcmotConfig[robotIdIn]->tp_vid);
//    return -1;
//    }


    emcmotDebug[robotIdIn] = &emcmotStruct->debug;

    emcmotPrimQueue[robotIdIn] = &emcmotStruct->debug.tp;     // primary motion queue
    emcmotAltQueue[robotIdIn] = &emcmotStruct->debug.altqueue;   // alternate motion queue

    // emcmotQueue: this was formerly &emcmotDebug->queue
    emcmotQueue[robotIdIn] = emcmotPrimQueue[robotIdIn];   // start on primary motion queue

    emcmotInternal[robotIdIn] = &emcmotStruct->internal;
    emcmotError[robotIdIn] = &emcmotStruct->error;

    /* init error struct */
    emcmotErrorInit(emcmotError[robotIdIn]);

    /* init command struct */
    emcmotCommand[robotIdIn]->head = 0;
//    emcmotCommand->command = 0;
    emcmotCommand[robotIdIn]->commandNum = 0;
    emcmotCommand[robotIdIn]->tail = 0;
    emcmotCommand[robotIdIn]->spindlesync = 0.0;

    /* init status struct */
    emcmotStatus[robotIdIn]->head = 0;
//    emcmotStatus->commandEcho = 0;
    emcmotStatus[robotIdIn]->commandNumEcho = 0;
//    emcmotStatus->commandStatus = 0;

    /* init more stuff */

    emcmotDebug[robotIdIn]->head = 0;
    emcmotConfig[robotIdIn]->head = 0;

    emcmotStatus[robotIdIn]->motionFlag = 0;
    SET_MOTION_ERROR_FLAG(0,emcmotStatus[robotIdIn]->motionFlag);
    SET_MOTION_COORD_FLAG(0,emcmotStatus[robotIdIn]->motionFlag);
    SET_MOTION_TELEOP_FLAG(0,emcmotStatus[robotIdIn]->motionFlag);
    emcmotDebug[robotIdIn]->split = 0;
    emcmotStatus[robotIdIn]->heartbeat = 0;
    emcmotStatus[robotIdIn]->computeTime = 0.0;
    emcmotConfig[robotIdIn]->numJoints = num_joints;

    ZERO_EMC_POSE(emcmotStatus[robotIdIn]->carte_pos_cmd);
    ZERO_EMC_POSE(emcmotStatus[robotIdIn]->carte_pos_fb);
    emcmotStatus[robotIdIn]->vel = VELOCITY;
    emcmotConfig[robotIdIn]->limitVel = VELOCITY;
    emcmotStatus[robotIdIn]->acc = ACCELERATION;
    emcmotStatus[robotIdIn]->feed_scale = 1.0;
    emcmotStatus[robotIdIn]->rapid_scale = 1.0;
    emcmotStatus[robotIdIn]->spindle_scale = 1.0;
    emcmotStatus[robotIdIn]->net_feed_scale = 1.0;
    /* adaptive feed is off by default, feed override, spindle
       override, and feed hold are on */
    emcmotStatus[robotIdIn]->enables_new = FS_ENABLED | SS_ENABLED | FH_ENABLED;
    emcmotStatus[robotIdIn]->enables_queued = emcmotStatus[robotIdIn]->enables_new;
    emcmotStatus[robotIdIn]->id = 0;
    emcmotStatus[robotIdIn]->depth = 0;
    emcmotStatus[robotIdIn]->activeDepth = 0;
    emcmotStatus[robotIdIn]->pause_state  = PS_RUNNING;
    emcmotStatus[robotIdIn]->resuming = 0;
    emcmotStatus[robotIdIn]->overrideLimitMask = 0;
    emcmotStatus[robotIdIn]->spindle.speed = 0.0;
    SET_MOTION_INPOS_FLAG(1,emcmotStatus[robotIdIn]->motionFlag);
    SET_MOTION_ENABLE_FLAG(0,emcmotStatus[robotIdIn]->motionFlag);
//    emcmotConfig[robotIdIn]->kinematics_type = kinType;

    emcmotDebug[robotIdIn]->oldPos = emcmotStatus[robotIdIn]->carte_pos_cmd;
    ZERO_EMC_POSE(emcmotDebug[robotIdIn]->oldVel);

    emcmot_config_change(robotIdIn);

    /* init pointer to joint structs */
    /* already initialized in init_hal_io, above */
#ifdef STRUCTS_IN_SHMEM
//    joints = &(emcmotDebug->joints[0]);
#else
    joints = &(joint_array[0]);
#endif

    /* init per-joint stuff */
    for (joint_num = 0; joint_num < num_joints; joint_num++)
    {
        /* point to structure for this joint */
        joint = &joints[joint_num+robotIdIn*EMCMOT_MAX_JOINTS];

        /* Export some HAL parameters */
//        retval = hal_pin_float_newf(HAL_IN, &(joint->home),
//                        mot_comp_id, "axis.%d.home", joint_num);
    //    if (retval != 0) {
    //        return retval;
    //    }

//        retval = hal_pin_float_newf(HAL_IN, &(joint->home_offset),
//                        mot_comp_id, "axis.%d.home-offset", joint_num);
    //    if (retval != 0) {
    //        return retval;
    //    }

        /* init the config fields with some "reasonable" defaults" */

        joint->type = 0;
        joint->max_pos_limit = 1.0;
        joint->min_pos_limit = -1.0;
        joint->vel_limit = 100.0;//影响轨迹抛物线插值的速度
        joint->acc_limit = 100.0;//影响轨迹抛物线插值的速度
        joint->min_ferror = 0.01;
        joint->max_ferror = 1.0;
        joint->home_search_vel = 0.0;
        joint->home_latch_vel = 0.0;
        joint->home_final_vel = -1;
//        joint->home_offset=(hal_float_t *)malloc(sizeof(float));
//        joint->home=(hal_float_t *)malloc(sizeof(float));
//        *(joint->home_offset) = 0.0;
//        *(joint->home) = 0.0;
        joint->home_flags = 0;
        joint->home_sequence = -1;
        joint->backlash = 0.0;

        joint->comp.entries = 0;
        joint->comp.entry = &(joint->comp.array[0]);
        /* the compensation code has -DBL_MAX at one end of the table
           and +DBL_MAX at the other so _all_ commanded positions are
           guaranteed to be covered by the table */
        joint->comp.array[0].nominal = -DBL_MAX;
        joint->comp.array[0].fwd_trim = 0.0;
        joint->comp.array[0].rev_trim = 0.0;
        joint->comp.array[0].fwd_slope = 0.0;
        joint->comp.array[0].rev_slope = 0.0;
        for ( n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) {
            joint->comp.array[n].nominal = DBL_MAX;
            joint->comp.array[n].fwd_trim = 0.0;
            joint->comp.array[n].rev_trim = 0.0;
            joint->comp.array[n].fwd_slope = 0.0;
            joint->comp.array[n].rev_slope = 0.0;
        }

        /* init status info */
        joint->flag = 0;
        joint->coarse_pos = 0.0;
        joint->pos_cmd = 0.0;
        joint->vel_cmd = 0.0;
        joint->backlash_corr = 0.0;
        joint->backlash_filt = 0.0;
        joint->backlash_vel = 0.0;
        joint->motor_pos_cmd = 0.0;
        joint->motor_pos_fb = 0.0;
        joint->pos_fb = 0.0;
        joint->ferror = 0.0;
        joint->ferror_limit = joint->min_ferror;
        joint->ferror_high_mark = 0.0;

        /* init internal info */
        CubicClass::cubicInit(&(joint->cubic));

        /* init misc other stuff in joint structure */
        joint->big_vel = 10.0 * joint->vel_limit;
    //    joint->home_state = 0;

        /* init joint flags (reduntant, since flag = 0 */

        SET_JOINT_ENABLE_FLAG(joint, 0);
        SET_JOINT_ACTIVE_FLAG(joint, 0);
        SET_JOINT_NHL_FLAG(joint, 0);
        SET_JOINT_PHL_FLAG(joint, 0);
        SET_JOINT_INPOS_FLAG(joint, 1);
        SET_JOINT_HOMING_FLAG(joint, 0);
        SET_JOINT_HOMED_FLAG(joint, 0);
        SET_JOINT_FERROR_FLAG(joint, 0);
        SET_JOINT_FAULT_FLAG(joint, 0);
        SET_JOINT_ERROR_FLAG(joint, 0);

    }

    /*! \todo FIXME-- add emcmotError */

    emcmotDebug[robotIdIn]->tMin = 0.0;
    emcmotDebug[robotIdIn]->tMax = 0.0;
    emcmotDebug[robotIdIn]->tAvg = 0.0;
    emcmotDebug[robotIdIn]->sMin = 0.0;
    emcmotDebug[robotIdIn]->sMax = 0.0;
    emcmotDebug[robotIdIn]->sAvg = 0.0;
    emcmotDebug[robotIdIn]->nMin = 0.0;
    emcmotDebug[robotIdIn]->nMax = 0.0;
    emcmotDebug[robotIdIn]->nAvg = 0.0;
    emcmotDebug[robotIdIn]->yMin = 0.0;
    emcmotDebug[robotIdIn]->yMax = 0.0;
    emcmotDebug[robotIdIn]->yAvg = 0.0;
    emcmotDebug[robotIdIn]->fyMin = 0.0;
    emcmotDebug[robotIdIn]->fyMax = 0.0;
    emcmotDebug[robotIdIn]->fyAvg = 0.0;
    emcmotDebug[robotIdIn]->fMin = 0.0;
    emcmotDebug[robotIdIn]->fMax = 0.0;
    emcmotDebug[robotIdIn]->fAvg = 0.0;

    emcmotDebug[robotIdIn]->cur_time = emcmotDebug[robotIdIn]->last_time = 0.0;
//    emcmotDebug->start_time = etime();
    emcmotDebug[robotIdIn]->running_time = 0.0;

    emcmotPrimQueue[robotIdIn] = &emcmotStruct->debug.tp;     // primary motion queue
    emcmotAltQueue[robotIdIn] = &emcmotStruct->debug.altqueue;   // alternate motion queue

    EmcMotionClass::initialEmcConfig(configIn,robotIdIn);

    // init the shared data between tp and using code
    emcmotDebug[robotIdIn]->tps = (tp_shared_t *)malloc(sizeof(tp_shared_t));
    if (!emcmotDebug[robotIdIn]->tps)
    {
        rtapi_print_msg(RTAPI_MSG_ERR,
                "MOTION: failed to create tp_shared in HAL memory");
        return -1;
    }
    init_shared(emcmotDebug[robotIdIn]->tps,
                emcmotConfig[robotIdIn],
                emcmotStatus[robotIdIn],
                emcmotDebug[robotIdIn],
                &joints[robotIdIn*EMCMOT_MAX_JOINTS], // internal joint data
                emcmot_hal_data); // HAL exorted part of joint data

    /* init motion emcmotDebug->queue */
    if (-1 == tpCreate(emcmotPrimQueue[robotIdIn], DEFAULT_TC_QUEUE_SIZE,
                      emcmotDebug[robotIdIn]->queueTcSpace,
                      emcmotDebug[robotIdIn]->tps))
    {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "MOTION: failed to create motion emcmotPrimQueue\n");
        return -1;
    }

    // and the alternate queue
    if (-1 == tpCreate(emcmotAltQueue[robotIdIn], DEFAULT_ALT_TC_QUEUE_SIZE,
                      emcmotDebug[robotIdIn]->altqueueTcSpace,
                      emcmotDebug[robotIdIn]->tps))
    {
        rtapi_print_msg(RTAPI_MSG_ERR,
            "MOTION: failed to create motion emcmotAltQueue\n");
        return -1;
    }



//    tpInit(&emcmotDebug->queue); // tpInit called from tpCreate
    tpSetCycleTime(emcmotQueue[robotIdIn], emcmotConfig[robotIdIn]->trajCycleTime);
    tpSetPos(emcmotQueue[robotIdIn], &emcmotStatus[robotIdIn]->carte_pos_cmd);
    tpSetVmax(emcmotQueue[robotIdIn], emcmotStatus[robotIdIn]->vel, emcmotStatus[robotIdIn]->vel);
    tpSetAmax(emcmotQueue[robotIdIn], emcmotStatus[robotIdIn]->acc);

    // the emcmotAltQueue parameters as per above are cloned
    // by tpSnapshot() during switching queues

    emcmotStatus[robotIdIn]->tail = 0;

//    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_comm_buffers() complete\n");
    return 0;
}

/* init_threads() creates realtime threads, exports functions to
   do the realtime control, and adds the functions to the threads.
*/
static int init_threads(int robotIdIn)
{
    double base_period_sec, servo_period_sec;
    int servo_base_ratio;
    int retval;

    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() starting...\n");
    int nothreads = ((base_period_nsec == 0) && (servo_period_nsec == 0));
    if (!nothreads) {

    /* if base_period not specified, assume same as servo_period */
    if (base_period_nsec == 0) {
        base_period_nsec = servo_period_nsec;
    }
    if (traj_period_nsec == 0) {
        traj_period_nsec = servo_period_nsec;
    }
    /* servo period must be greater or equal to base period */
    if (servo_period_nsec < base_period_nsec) {
        rtapi_print_msg(RTAPI_MSG_ERR,
                "MOTION: bad servo period %ld nsec\n", servo_period_nsec);
        return -1;
    }
    /* convert desired periods to floating point */
    base_period_sec = base_period_nsec * 0.000000001;
    servo_period_sec = servo_period_nsec * 0.000000001;
    /* calculate period ratios, round to nearest integer */
    servo_base_ratio = (servo_period_sec / base_period_sec) + 0.5;
    /* revise desired periods to be integer multiples of each other */
    servo_period_nsec = base_period_nsec * servo_base_ratio;
    /* create HAL threads for each period */
    /* only create base thread if it is faster than servo thread */
    if (servo_base_ratio > 1) {

//        retval = hal_create_thread("base-thread", base_period_nsec, base_thread_fp, base_cpu);
//        if (retval < 0) {
//        rtapi_print_msg(RTAPI_MSG_ERR,
//                "MOTION: failed to create %ld nsec base thread\n",
//                base_period_nsec);
//        return -1;
        }
    }
//    retval = hal_create_thread("servo-thread", servo_period_nsec, 1, servo_cpu);
//    if (retval < 0) {
//        rtapi_print_msg(RTAPI_MSG_ERR,
//                "MOTION: failed to create %ld nsec servo thread\n",
//                servo_period_nsec);
//        return -1;
//    }
//    } else {
//    rtapi_print_msg(RTAPI_MSG_INFO,
//            "MOTION: not creating threads as both servo and base period are 0\n");
//    }
    /* export realtime functions that do the real work */
//    hal_export_xfunct_args_t mot_args = {
//        .type = FS_XTHREADFUNC,
//        .funct.x = emcmotController,
//        .arg = 0,
//        .uses_fp = 1,
//        .reentrant = 0,
//        .owner_id = mot_comp_id
//    };

//    retval = hal_export_xfunctf(&mot_args, "motion-controller");
    if (retval < 0) {
    rtapi_print_msg(RTAPI_MSG_ERR,
        "MOTION: failed to export controller function\n");
    return -1;
    }
//    hal_export_xfunct_args_t cmd_args = {
//        .type = FS_XTHREADFUNC,
//        .funct.x = emcmotCommandHandler,
//        .arg = 0,
//        .uses_fp = 1,
//        .reentrant = 0,
//        .owner_id = mot_comp_id
//    };
//    retval = hal_export_xfunctf(&cmd_args, "motion-command-handler");
    if (retval < 0) {
    rtapi_print_msg(RTAPI_MSG_ERR,
        "MOTION: failed to export command handler function\n");
    return -1;
    }
/*! \todo Another #if 0 */
#if 0
    /*! \todo FIXME - currently the traj planner is called from the controller */
    /* eventually it will be a separate function */
    retval = hal_export_funct("motion-traj-planner", emcmotTrajPlanner, 0	/* arg
     */ , 1 /* uses_fp */ ,
    0 /* reentrant */ , mot_comp_id);
    if (retval < 0) {
    rtapi_print_msg(RTAPI_MSG_ERR,
        "MOTION: failed to export traj planner function\n");
    return -1;
    }
#endif
    if (!nothreads) {
    // if we don't set cycle times based on these guesses, emc doesn't
    // start up right
    // if no threads, postponed until first invocation of emcmotCommandHandler
    EmcMotionClass::setServoCycleTime(servo_period_nsec * 1e-9,robotIdIn);
    EmcMotionClass::setTrajCycleTime(traj_period_nsec * 1e-9,robotIdIn);
    }
    rtapi_print_msg(RTAPI_MSG_INFO, "MOTION: init_threads() complete\n");
    return 0;
}

void EmcMotionClass::emcmotSetCycleTime(unsigned long nsec ,int robotIdIn) {
    int servo_mult;
    servo_mult = traj_period_nsec / nsec;
    if(servo_mult < 0) servo_mult = 1;
    setTrajCycleTime(nsec * 1e-9,robotIdIn);
    setServoCycleTime(nsec * servo_mult * 1e-9,robotIdIn);
}
/* call this when setting the trajectory cycle time */
int EmcMotionClass::setTrajCycleTime(double secs,int robotIdIn)
{
    static int t;

    rtapi_print_msg(RTAPI_MSG_INFO,
    "MOTION: setting Traj cycle time to %ld nsecs\n", (long) (secs * 1e9));

    /* make sure it's not zero */
    if (secs <= 0.0) {

    return -1;
    }

    emcmot_config_change(robotIdIn);

    /* compute the interpolation rate as nearest integer to traj/servo */
    if(emcmotConfig[robotIdIn]->servoCycleTime)
        emcmotConfig[robotIdIn]->interpolationRate =
            (int) (secs / emcmotConfig[robotIdIn]->servoCycleTime + 0.5);
    else
        emcmotConfig[robotIdIn]->interpolationRate = 1;

    /* set traj planner */
    tpSetCycleTime(emcmotPrimQueue[robotIdIn], secs);

    /* set the free planners, cubic interpolation rate and segment time */
    for (t = 0; t < num_joints; t++) {
    CubicClass::cubicSetInterpolationRate(&(joints[t+robotIdIn*EMCMOT_MAX_JOINTS].cubic),
        emcmotConfig[robotIdIn]->interpolationRate);
    }

    /* copy into status out */
    emcmotConfig[robotIdIn]->trajCycleTime = secs;

    return 0;
}

/* call this when setting the servo cycle time */
int EmcMotionClass::setServoCycleTime(double secs,int robotIdIn)
{
    static int t;

    rtapi_print_msg(RTAPI_MSG_INFO,
    "MOTION: setting Servo cycle time to %ld nsecs\n", (long) (secs * 1e9));

    /* make sure it's not zero */
    if (secs <= 0.0) {
    return -1;
    }

    emcmot_config_change(robotIdIn);

    /* compute the interpolation rate as nearest integer to traj/servo */
    emcmotConfig[robotIdIn]->interpolationRate =
    (int) (emcmotConfig[robotIdIn]->trajCycleTime / secs + 0.5);

    /* set the cubic interpolation rate and PID cycle time */
    for (t = 0; t < num_joints; t++) {
    CubicClass::cubicSetInterpolationRate(&(joints[t+robotIdIn*EMCMOT_MAX_JOINTS].cubic),
        emcmotConfig[robotIdIn]->interpolationRate);
    CubicClass::cubicSetSegmentTime(&(joints[t+robotIdIn*EMCMOT_MAX_JOINTS].cubic), secs);
    }

    /* copy into status out */
    emcmotConfig[robotIdIn]->servoCycleTime = secs;

    return 0;
}

int EmcMotionClass::initialEmcConfig(struct emcmot_config_t configIn , int robotIdIn)
{
    *emcmotConfig[robotIdIn]=configIn;
//    *emcmotConfig[robotIdIn]=g_emcmotConfig[robotIdIn]_outSide;
//    emcmotConfig[robotIdIn]->trajCycleTime=0.001;

//    // from emcmotConfig[robotIdIn]
//    emcmotConfig[robotIdIn]->arcBlendGapCycles=3;
//    emcmotConfig[robotIdIn]->arcBlendOptDepth=3;
//    emcmotConfig[robotIdIn]->arcBlendEnable=1;
//    emcmotConfig[robotIdIn]->arcBlendRampFreq=1;
//    emcmotConfig[robotIdIn]->arcBlendTangentKinkRatio=1;
//    emcmotConfig[robotIdIn]->arcBlendFallbackEnable=0;
    //    emcmotConfig[robotIdIn]->maxFeedScale=10;
}

int EmcMotionClass::setJointConfig(int robotIdIn)
{
    emcmot_joint_t *joint;

    for (int joint_num = 0; joint_num < num_joints; joint_num++)
    {

        joint = &joints[joint_num+robotIdIn*EMCMOT_MAX_JOINTS];

        joint->type = 0;
        joint->max_pos_limit = 1.0;
        joint->min_pos_limit = -1.0;
        joint->vel_limit = 10.0;//影响轨迹hualei
        joint->acc_limit = 10.0;//影响轨迹
        joint->min_ferror = 0.01;
        joint->max_ferror = 1.0;
        joint->home_search_vel = 0.0;
        joint->home_latch_vel = 0.0;
        joint->home_final_vel = -1;
//        joint->home_offset=(hal_float_t *)malloc(sizeof(float));
//        joint->home=(hal_float_t *)malloc(sizeof(float));
//        *(joint->home_offset) = 0.0;
//        *(joint->home) = 0.0;
        joint->home_flags = 0;
        joint->home_sequence = -1;
        joint->backlash = 0.0;

        joint->comp.entries = 0;
        joint->comp.entry = &(joint->comp.array[0]);
        /* the compensation code has -DBL_MAX at one end of the table
           and +DBL_MAX at the other so _all_ commanded positions are
           guaranteed to be covered by the table */
        joint->comp.array[0].nominal = -DBL_MAX;
        joint->comp.array[0].fwd_trim = 0.0;
        joint->comp.array[0].rev_trim = 0.0;
        joint->comp.array[0].fwd_slope = 0.0;
        joint->comp.array[0].rev_slope = 0.0;
        for (int n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) {
            joint->comp.array[n].nominal = DBL_MAX;
            joint->comp.array[n].fwd_trim = 0.0;
            joint->comp.array[n].rev_trim = 0.0;
            joint->comp.array[n].fwd_slope = 0.0;
            joint->comp.array[n].rev_slope = 0.0;
        }

        /* init status info */
        joint->flag = 0;
        joint->coarse_pos = 0.0;
        joint->pos_cmd = 0.0;
        joint->vel_cmd = 0.0;
        joint->backlash_corr = 0.0;
        joint->backlash_filt = 0.0;
        joint->backlash_vel = 0.0;
        joint->motor_pos_cmd = 0.0;
        joint->motor_pos_fb = 0.0;
        joint->pos_fb = 0.0;
        joint->ferror = 0.0;
        joint->ferror_limit = joint->min_ferror;
        joint->ferror_high_mark = 0.0;

        /* init internal info */
//        CubicClass::cubicInit(&(joint->cubic));

        /* init misc other stuff in joint structure */
        joint->big_vel = 10.0 * joint->vel_limit;
    //    joint->home_state = 0;

        /* init joint flags (reduntant, since flag = 0 */

        SET_JOINT_ENABLE_FLAG(joint, 0);
        SET_JOINT_ACTIVE_FLAG(joint, 0);
        SET_JOINT_NHL_FLAG(joint, 0);
        SET_JOINT_PHL_FLAG(joint, 0);
        SET_JOINT_INPOS_FLAG(joint, 1);
        SET_JOINT_HOMING_FLAG(joint, 0);
        SET_JOINT_HOMED_FLAG(joint, 0);
        SET_JOINT_FERROR_FLAG(joint, 0);
        SET_JOINT_FAULT_FLAG(joint, 0);
        SET_JOINT_ERROR_FLAG(joint, 0);

    }
    return 1;

}

int EmcMotionClass::setJointVel(int jointNum, double maxVel, double maxAcc, int robotIdIn)
{

    if(num_joints<=jointNum)
    {
        return 0;
    }
    emcmot_joint_t *joint;
    joint = &joints[jointNum+robotIdIn*EMCMOT_MAX_JOINTS];
    joint->vel_limit = maxVel;
    joint->acc_limit = maxAcc;
    joint->simpleTp.max_vel=maxVel;
    joint->simpleTp.max_acc=maxAcc;
//    std::cout<<"~~~~~~~~~~~~~~~~~~~setJointVel jointNum"<<jointNum<<" maxVel"<<maxVel<<" maxAcc"<<maxAcc<<" robotIdIn"<<robotIdIn
//            <<"num_joints"<<num_joints<<"out "<<joint->vel_limit<<std::endl;
//    for (int joint_num = 0; joint_num < num_joints; joint_num++)
//    {



//        joint->type = 0;
//        joint->max_pos_limit = 1.0;
//        joint->min_pos_limit = -1.0;

////        joint->acc_limit = maxAcc;
//        joint->min_ferror = 0.01;
//        joint->max_ferror = 1.0;
//        joint->home_search_vel = 0.0;
//        joint->home_latch_vel = 0.0;
//        joint->home_final_vel = -1;
//        joint->home_offset=(hal_float_t *)malloc(sizeof(float));
//        joint->home=(hal_float_t *)malloc(sizeof(float));
//        *(joint->home_offset) = 0.0;
//        *(joint->home) = 0.0;
//        joint->home_flags = 0;
//        joint->home_sequence = -1;
//        joint->backlash = 0.0;

//        joint->comp.entries = 0;
//        joint->comp.entry = &(joint->comp.array[0]);
//        /* the compensation code has -DBL_MAX at one end of the table
//           and +DBL_MAX at the other so _all_ commanded positions are
//           guaranteed to be covered by the table */
//        joint->comp.array[0].nominal = -DBL_MAX;
//        joint->comp.array[0].fwd_trim = 0.0;
//        joint->comp.array[0].rev_trim = 0.0;
//        joint->comp.array[0].fwd_slope = 0.0;
//        joint->comp.array[0].rev_slope = 0.0;
//        for (int n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) {
//            joint->comp.array[n].nominal = DBL_MAX;
//            joint->comp.array[n].fwd_trim = 0.0;
//            joint->comp.array[n].rev_trim = 0.0;
//            joint->comp.array[n].fwd_slope = 0.0;
//            joint->comp.array[n].rev_slope = 0.0;
//        }

//        /* init status info */
//        joint->flag = 0;
//        joint->coarse_pos = 0.0;
//        joint->pos_cmd = 0.0;
//        joint->vel_cmd = 0.0;
//        joint->backlash_corr = 0.0;
//        joint->backlash_filt = 0.0;
//        joint->backlash_vel = 0.0;
//        joint->motor_pos_cmd = 0.0;
//        joint->motor_pos_fb = 0.0;
//        joint->pos_fb = 0.0;
//        joint->ferror = 0.0;
//        joint->ferror_limit = joint->min_ferror;
//        joint->ferror_high_mark = 0.0;

//        /* init internal info */
////        CubicClass::cubicInit(&(joint->cubic));

//        /* init misc other stuff in joint structure */
//        joint->big_vel = 10.0 * joint->vel_limit;
//    //    joint->home_state = 0;

//        /* init joint flags (reduntant, since flag = 0 */

//        SET_JOINT_ENABLE_FLAG(joint, 0);
//        SET_JOINT_ACTIVE_FLAG(joint, 0);
//        SET_JOINT_NHL_FLAG(joint, 0);
//        SET_JOINT_PHL_FLAG(joint, 0);
//        SET_JOINT_INPOS_FLAG(joint, 1);
//        SET_JOINT_HOMING_FLAG(joint, 0);
//        SET_JOINT_HOMED_FLAG(joint, 0);
//        SET_JOINT_FERROR_FLAG(joint, 0);
//        SET_JOINT_FAULT_FLAG(joint, 0);
//        SET_JOINT_ERROR_FLAG(joint, 0);

//    }

    return 1;
}

int EmcMotionClass::setBoundVel(int jointNum, double maxVel, double maxAcc, int robotIdIn)
{
    if(num_joints<=jointNum)
    {
        return 0;
    }
    emcmot_joint_t *joint;
    joint = &joints[jointNum+robotIdIn*EMCMOT_MAX_JOINTS];
    joint->bound_vel_limit = maxVel;
    joint->bound_acc_limit = maxAcc;
    return 1;

}

//int EmcMotionClass::setJointAcc(double maxAcc)
//{
//    emcmot_joint_t *joint;

//    for (int joint_num = 0; joint_num < num_joints; joint_num++)
//    {

//        joint = &joints[joint_num];

//        joint->type = 0;
//        joint->max_pos_limit = 1.0;
//        joint->min_pos_limit = -1.0;
////        joint->vel_limit = maxVel;//影响轨迹hualei 抛物线插值速度
//        joint->acc_limit = maxAcc;//影响轨迹
//        joint->min_ferror = 0.01;
//        joint->max_ferror = 1.0;
//        joint->home_search_vel = 0.0;
//        joint->home_latch_vel = 0.0;
//        joint->home_final_vel = -1;
//        joint->home_offset=(hal_float_t *)malloc(sizeof(float));
//        joint->home=(hal_float_t *)malloc(sizeof(float));
//        *(joint->home_offset) = 0.0;
//        *(joint->home) = 0.0;
//        joint->home_flags = 0;
//        joint->home_sequence = -1;
//        joint->backlash = 0.0;

//        joint->comp.entries = 0;
//        joint->comp.entry = &(joint->comp.array[0]);
//        /* the compensation code has -DBL_MAX at one end of the table
//           and +DBL_MAX at the other so _all_ commanded positions are
//           guaranteed to be covered by the table */
//        joint->comp.array[0].nominal = -DBL_MAX;
//        joint->comp.array[0].fwd_trim = 0.0;
//        joint->comp.array[0].rev_trim = 0.0;
//        joint->comp.array[0].fwd_slope = 0.0;
//        joint->comp.array[0].rev_slope = 0.0;
//        for (int n = 1 ; n < EMCMOT_COMP_SIZE+2 ; n++ ) {
//            joint->comp.array[n].nominal = DBL_MAX;
//            joint->comp.array[n].fwd_trim = 0.0;
//            joint->comp.array[n].rev_trim = 0.0;
//            joint->comp.array[n].fwd_slope = 0.0;
//            joint->comp.array[n].rev_slope = 0.0;
//        }

//        /* init status info */
//        joint->flag = 0;
//        joint->coarse_pos = 0.0;
//        joint->pos_cmd = 0.0;
//        joint->vel_cmd = 0.0;
//        joint->backlash_corr = 0.0;
//        joint->backlash_filt = 0.0;
//        joint->backlash_vel = 0.0;
//        joint->motor_pos_cmd = 0.0;
//        joint->motor_pos_fb = 0.0;
//        joint->pos_fb = 0.0;
//        joint->ferror = 0.0;
//        joint->ferror_limit = joint->min_ferror;
//        joint->ferror_high_mark = 0.0;

//        /* init internal info */
////        CubicClass::cubicInit(&(joint->cubic));

//        /* init misc other stuff in joint structure */
//        joint->big_vel = 10.0 * joint->vel_limit;
//    //    joint->home_state = 0;

//        /* init joint flags (reduntant, since flag = 0 */

//        SET_JOINT_ENABLE_FLAG(joint, 0);
//        SET_JOINT_ACTIVE_FLAG(joint, 0);
//        SET_JOINT_NHL_FLAG(joint, 0);
//        SET_JOINT_PHL_FLAG(joint, 0);
//        SET_JOINT_INPOS_FLAG(joint, 1);
//        SET_JOINT_HOMING_FLAG(joint, 0);
//        SET_JOINT_HOMED_FLAG(joint, 0);
//        SET_JOINT_FERROR_FLAG(joint, 0);
//        SET_JOINT_FAULT_FLAG(joint, 0);
//        SET_JOINT_ERROR_FLAG(joint, 0);

//    }
//    return 1;
//}

static int init_shared(tp_shared_t *tps,
               struct emcmot_config_t *cfg,
               struct emcmot_status_t *status,
               emcmot_debug_t *dbg,
               emcmot_joint_t *joint,
               emcmot_hal_data_t *hal) // hal not used yet
{
    // global module param
    tps->num_dio = &num_dio;
    tps->num_aio = &num_aio;

    // from emcmotConfig[robotIdIn]
    tps->arcBlendGapCycles = &cfg->arcBlendGapCycles;
    tps->arcBlendOptDepth = &cfg->arcBlendOptDepth;
    tps->arcBlendEnable = &cfg->arcBlendEnable;
    tps->arcBlendRampFreq = &cfg->arcBlendRampFreq;
    tps->arcBlendTangentKinkRatio = &cfg->arcBlendTangentKinkRatio;
    tps->arcBlendFallbackEnable = &cfg->arcBlendFallbackEnable;
    tps->maxFeedScale = &cfg->maxFeedScale;

    // from emcmotStatus
    tps->net_feed_scale = &status->net_feed_scale;
    tps->spindle_direction = &status->spindle.direction;
    tps->spindle_speed = &status->spindle.speed;
    tps->spindleRevs = &status->spindleRevs;
    tps->spindleSpeedIn = &status->spindleSpeedIn;
    tps->spindle_index_enable = &status->spindle_index_enable;
    tps->spindle_is_atspeed = &status->spindle_is_atspeed; // or pin
    tps->spindleSync = &status->spindleSync;
    tps->current_vel = &status->current_vel;
    tps->requested_vel = &status->requested_vel;
    tps->distance_to_go = &status->distance_to_go;
    tps->enables_new = &status->enables_new;
    tps->enables_queued = &status->enables_queued;
    tps->tcqlen = &status->tcqlen;

    tps->dtg[0] = &status->dtg.tran.x;
    tps->dtg[1] = &status->dtg.tran.y;
    tps->dtg[2] = &status->dtg.tran.z;
    tps->dtg[3] = &status->dtg.a;
    tps->dtg[4] = &status->dtg.b;
    tps->dtg[5] = &status->dtg.c;
    tps->dtg[6] = &status->dtg.u;
    tps->dtg[7] = &status->dtg.v;
    tps->dtg[8] = &status->dtg.w;

    // from joints array 用于拐角平滑处理计算
    tps->acc_limit[0] = &joint[0].bound_acc_limit;
    tps->acc_limit[1] = &joint[1].bound_acc_limit;
    tps->acc_limit[2] = &joint[2].bound_acc_limit;

    tps->vel_limit[0] = &joint[0].bound_vel_limit;
    tps->vel_limit[1] = &joint[1].bound_vel_limit;
    tps->vel_limit[2] = &joint[2].bound_vel_limit;

    // from  emcmot_debug_t
    tps->stepping = &dbg->stepping;

    // M6x pin setters
    tps->dioWrite = emcmotDioWrite;
    tps->aioWrite = emcmotAioWrite;

    // rotary setter/getters
    tps->SetRotaryUnlock = emcmotSetRotaryUnlock;
    tps->GetRotaryIsUnlocked = emcmotGetRotaryIsUnlocked;
    return 0;
}
